
// import haxe.ds.Array;
// import oimo.dynamics.constraint.contact.ContactConstraint;
// import oimo.dynamics.constraint.contact.ContactImpulse;
// import oimo.dynamics.constraint.info.contact.ContactSolverInfo;
// import oimo.dynamics.constraint.info.contact.ContactSolverInfoRow;
// import oimo.m.Mat3;
// import oimo.m.Vec3;
// import oimo.m.M;
// import oimo.common.MathUtil;
// import oimo.common.Setting;
// import oimo.dynamics.TimeStep;
// import oimo.dynamics.constraint.ConstraintSolver;
// import oimo.dynamics.constraint.contact.*;
// import oimo.dynamics.constraint.info.JacobianRow;
// import oimo.dynamics.constraint.info.contact.*;
// import oimo.dynamics.constraint.solver.common.ContactSolverMassDataRow;

import { ConstraintSolver } from "../../ConstraintSolver";
import { ContactConstraint } from "../../contact/ContactConstraint";
import { ContactSolverInfo } from "../../info/contact/ContactSolverInfo";
import { Setting } from "../../../../common/Setting";
import { TimeStep } from "../../../TimeStep";
import { Mat3 } from "../../../../common/Mat3";
import { M } from "../../../../common/M";
import { ContactSolverInfoRow } from "../../info/contact/ContactSolverInfoRow";
import { JacobianRow } from "../../info/JacobianRow";
import { Vec3 } from "../../../../common/Vec3";
import { ContactImpulse } from "../../contact/ContactImpulse";
import { MathUtil } from "../../../../common/MathUtil";
import { ContactSolverMassDataRow } from "../common/ContactSolverMassDataRow";

/**
 * A contact constraint solver using projected Gauss-Seidel (sequential impulse).
 */

export class PgsContactConstraintSolver extends ConstraintSolver {
	constraint: ContactConstraint;

	info: ContactSolverInfo;

	massData: Array<ContactSolverMassDataRow>;


	constructor(constraint: ContactConstraint) {
		super();
		this.constraint = constraint;

		this.info = new ContactSolverInfo();

		this.massData = new Array<ContactSolverMassDataRow>(Setting.maxManifoldPoints);

		for (let i = 0; i < this.massData.length; ++i) {
			this.massData[i] = new ContactSolverMassDataRow();
		}
	}

	public preSolveVelocity(timeStep: TimeStep): void {
		this.constraint._getVelocitySolverInfo(timeStep, this.info);

		this._b1 = this.info.b1;
		this._b2 = this.info.b2;

		var invM1: number = this._b1._invMass;
		var invM2: number = this._b2._invMass;

		var invI1: Mat3 = this._b1._invInertia.clone();
		var invI2: Mat3 = this._b2._invInertia.clone();

		// compute mass data
		for (let i = 0; i < this.info.numRows; ++i) {
			var row: ContactSolverInfoRow = this.info.rows[i];
			var md: ContactSolverMassDataRow = this.massData[i];
			var j: JacobianRow;

			// normal mass
			j = row.jacobianN;
			M.vec3_scale(md.invMLinN1, j.lin1, invM1);
			M.vec3_scale(md.invMLinN2, j.lin2, invM2);
			M.vec3_mulMat3(md.invMAngN1, j.ang1, invI1);
			M.vec3_mulMat3(md.invMAngN2, j.ang2, invI2);

			md.massN = invM1 + invM2 + M.vec3_dot(md.invMAngN1, j.ang1) + M.vec3_dot(md.invMAngN2, j.ang2);
			if (md.massN != 0) md.massN = 1 / md.massN;

			// tangent/binormal mass
			var jt: JacobianRow = row.jacobianT;
			var jb: JacobianRow = row.jacobianB;
			M.vec3_scale(md.invMLinT1, jt.lin1, invM1);
			M.vec3_scale(md.invMLinT2, jt.lin2, invM2);
			M.vec3_scale(md.invMLinB1, jb.lin1, invM1);
			M.vec3_scale(md.invMLinB2, jb.lin2, invM2);
			M.vec3_mulMat3(md.invMAngT1, jt.ang1, invI1);
			M.vec3_mulMat3(md.invMAngT2, jt.ang2, invI2);
			M.vec3_mulMat3(md.invMAngB1, jb.ang1, invI1);
			M.vec3_mulMat3(md.invMAngB2, jb.ang2, invI2);

			// compute effective mass matrix for friction
			var invMassTB00: number = invM1 + invM2 + M.vec3_dot(md.invMAngT1, jt.ang1) + M.vec3_dot(md.invMAngT2, jt.ang2);
			var invMassTB01: number = M.vec3_dot(md.invMAngT1, jb.ang1) + M.vec3_dot(md.invMAngT2, jb.ang2);
			var invMassTB10: number = invMassTB01;
			var invMassTB11: number = invM1 + invM2 + M.vec3_dot(md.invMAngB1, jb.ang1) + M.vec3_dot(md.invMAngB2, jb.ang2);

			var invDet: number = invMassTB00 * invMassTB11 - invMassTB01 * invMassTB10;
			if (invDet != 0) invDet = 1 / invDet;

			md.massTB00 = invMassTB11 * invDet;
			md.massTB01 = -invMassTB01 * invDet;
			md.massTB10 = -invMassTB10 * invDet;
			md.massTB11 = invMassTB00 * invDet;
		}
	}

	public warmStart(timeStep: TimeStep): void {
		var lv1: Vec3 = new Vec3();
		var lv2: Vec3 = new Vec3();
		var av1: Vec3 = new Vec3();
		var av2: Vec3 = new Vec3();
		M.vec3_assign(lv1, this._b1._vel);
		M.vec3_assign(lv2, this._b2._vel);
		M.vec3_assign(av1, this._b1._angVel);
		M.vec3_assign(av2, this._b2._angVel);

		for (let i = 0; i < this.info.numRows; ++i) {
			var row: ContactSolverInfoRow = this.info.rows[i];
			var imp: ContactImpulse = row.impulse;
			var md: ContactSolverMassDataRow = this.massData[i];
			var jt: JacobianRow = row.jacobianT;
			var jb: JacobianRow = row.jacobianB;

			var impulseN: number = imp.impulseN;
			var impulseT: number = M.vec3_dot(imp.impulseL, jt.lin1);
			var impulseB: number = M.vec3_dot(imp.impulseL, jb.lin1);
			imp.impulseT = impulseT;
			imp.impulseB = impulseB;

			// adjust impulse for variable time step
			imp.impulseN *= timeStep.dtRatio;
			imp.impulseT *= timeStep.dtRatio;
			imp.impulseB *= timeStep.dtRatio;

			M.vec3_addRhsScaledEQ(lv1, lv1, md.invMLinN1, impulseN);
			M.vec3_addRhsScaledEQ(lv1, lv1, md.invMLinT1, impulseT);
			M.vec3_addRhsScaledEQ(lv1, lv1, md.invMLinB1, impulseB);
			M.vec3_addRhsScaledEQ(lv2, lv2, md.invMLinN2, -impulseN);
			M.vec3_addRhsScaledEQ(lv2, lv2, md.invMLinT2, -impulseT);
			M.vec3_addRhsScaledEQ(lv2, lv2, md.invMLinB2, -impulseB);
			M.vec3_addRhsScaledEQ(av1, av1, md.invMAngN1, impulseN);
			M.vec3_addRhsScaledEQ(av1, av1, md.invMAngT1, impulseT);
			M.vec3_addRhsScaledEQ(av1, av1, md.invMAngB1, impulseB);
			M.vec3_addRhsScaledEQ(av2, av2, md.invMAngN2, -impulseN);
			M.vec3_addRhsScaledEQ(av2, av2, md.invMAngT2, -impulseT);
			M.vec3_addRhsScaledEQ(av2, av2, md.invMAngB2, -impulseB);
		}

		M.vec3_assign(this._b1._vel, lv1);
		M.vec3_assign(this._b2._vel, lv2);
		M.vec3_assign(this._b1._angVel, av1);
		M.vec3_assign(this._b2._angVel, av2);
	}

	public solveVelocity(): void {
		var lv1: Vec3 = new Vec3();
		var lv2: Vec3 = new Vec3();
		var av1: Vec3 = new Vec3();
		var av2: Vec3 = new Vec3();
		M.vec3_assign(lv1, this._b1._vel);
		M.vec3_assign(lv2, this._b2._vel);
		M.vec3_assign(av1, this._b1._angVel);
		M.vec3_assign(av2, this._b2._angVel);

		// solve friction
		for (let i = 0; i < this.info.numRows; ++i) {
			var row: ContactSolverInfoRow = this.info.rows[i];
			var md: ContactSolverMassDataRow = this.massData[i];
			var imp: ContactImpulse = row.impulse;
			var j: JacobianRow;

			// measure relative velocity
			var rvt: number = 0;
			j = row.jacobianT;
			rvt += M.vec3_dot(lv1, j.lin1);
			rvt -= M.vec3_dot(lv2, j.lin2);
			rvt += M.vec3_dot(av1, j.ang1);
			rvt -= M.vec3_dot(av2, j.ang2);

			var rvb: number = 0;
			j = row.jacobianB;
			rvb += M.vec3_dot(lv1, j.lin1);
			rvb -= M.vec3_dot(lv2, j.lin2);
			rvb += M.vec3_dot(av1, j.ang1);
			rvb -= M.vec3_dot(av2, j.ang2);

			var impulseT: number = -(rvt * md.massTB00 + rvb * md.massTB01);
			var impulseB: number = -(rvt * md.massTB10 + rvb * md.massTB11);

			var oldImpulseT: number = imp.impulseT;
			var oldImpulseB: number = imp.impulseB;
			imp.impulseT += impulseT;
			imp.impulseB += impulseB;

			// cone friction
			var maxImpulse: number = row.friction * imp.impulseN;
			if (maxImpulse == 0) {
				imp.impulseT = 0;
				imp.impulseB = 0;
			} else {
				var impulseLengthSq: number = imp.impulseT * imp.impulseT + imp.impulseB * imp.impulseB;
				if (impulseLengthSq > maxImpulse * maxImpulse) {
					var invL: number = maxImpulse / MathUtil.sqrt(impulseLengthSq);
					imp.impulseT *= invL;
					imp.impulseB *= invL;
				}
			}

			impulseT = imp.impulseT - oldImpulseT;
			impulseB = imp.impulseB - oldImpulseB;

			// apply delta impulse
			M.vec3_addRhsScaledEQ(lv1, lv1, md.invMLinT1, impulseT);
			M.vec3_addRhsScaledEQ(lv1, lv1, md.invMLinB1, impulseB);
			M.vec3_addRhsScaledEQ(lv2, lv2, md.invMLinT2, -impulseT);
			M.vec3_addRhsScaledEQ(lv2, lv2, md.invMLinB2, -impulseB);
			M.vec3_addRhsScaledEQ(av1, av1, md.invMAngT1, impulseT);
			M.vec3_addRhsScaledEQ(av1, av1, md.invMAngB1, impulseB);
			M.vec3_addRhsScaledEQ(av2, av2, md.invMAngT2, -impulseT);
			M.vec3_addRhsScaledEQ(av2, av2, md.invMAngB2, -impulseB);
		}

		// solve normal
		for (let i = 0; i < this.info.numRows; ++i) {
			var row: ContactSolverInfoRow = this.info.rows[i];
			var md: ContactSolverMassDataRow = this.massData[i];
			var imp: ContactImpulse = row.impulse;
			var j: JacobianRow;

			// measure relative velocity
			var rvn: number = 0;
			j = row.jacobianN;
			rvn += M.vec3_dot(lv1, j.lin1);
			rvn -= M.vec3_dot(lv2, j.lin2);
			rvn += M.vec3_dot(av1, j.ang1);
			rvn -= M.vec3_dot(av2, j.ang2);

			var impulseN: number = (row.rhs - rvn) * md.massN;

			// clamp impulse
			var oldImpulseN: number = imp.impulseN;
			imp.impulseN += impulseN;
			if (imp.impulseN < 0) imp.impulseN = 0;
			impulseN = imp.impulseN - oldImpulseN;

			// apply delta impulse
			M.vec3_addRhsScaledEQ(lv1, lv1, md.invMLinN1, impulseN);
			M.vec3_addRhsScaledEQ(lv2, lv2, md.invMLinN2, -impulseN);
			M.vec3_addRhsScaledEQ(av1, av1, md.invMAngN1, impulseN);
			M.vec3_addRhsScaledEQ(av2, av2, md.invMAngN2, -impulseN);
		}

		M.vec3_assign(this._b1._vel, lv1);
		M.vec3_assign(this._b2._vel, lv2);
		M.vec3_assign(this._b1._angVel, av1);
		M.vec3_assign(this._b2._angVel, av2);
	}


	private updatePositionData(): void {
		this.constraint._syncManifold();
		this.constraint._getPositionSolverInfo(this.info);

		var invM1: number = this._b1._invMass;
		var invM2: number = this._b2._invMass;

		var invI1: Mat3 = this._b1._invInertia.clone();
		var invI2: Mat3 = this._b2._invInertia.clone();

		// compute mass data
		for (let i = 0; i < this.info.numRows; ++i) {
			var row: ContactSolverInfoRow = this.info.rows[i];
			var md: ContactSolverMassDataRow = this.massData[i];
			var j: JacobianRow = row.jacobianN;

			M.vec3_scale(md.invMLinN1, j.lin1, invM1);
			M.vec3_scale(md.invMLinN2, j.lin2, invM2);
			M.vec3_mulMat3(md.invMAngN1, j.ang1, invI1);
			M.vec3_mulMat3(md.invMAngN2, j.ang2, invI2);

			md.massN = invM1 + invM2 + M.vec3_dot(md.invMAngN1, j.ang1) + M.vec3_dot(md.invMAngN2, j.ang2);
			if (md.massN != 0) md.massN = 1 / md.massN;
		}
	}

	public preSolvePosition(timeStep: TimeStep): void {
		this.updatePositionData();

		// clear position impulses
		for (let i = 0; i < this.info.numRows; ++i) {
			this.info.rows[i].impulse.impulseP = 0;
		}
	}

	public solvePositionSplitImpulse(): void {
		var lv1: Vec3 = new Vec3();
		var lv2: Vec3 = new Vec3();
		var av1: Vec3 = new Vec3();
		var av2: Vec3 = new Vec3();
		M.vec3_assign(lv1, this._b1._pseudoVel);
		M.vec3_assign(lv2, this._b2._pseudoVel);
		M.vec3_assign(av1, this._b1._angPseudoVel);
		M.vec3_assign(av2, this._b2._angPseudoVel);

		// solve normal
		for (let i = 0; i < this.info.numRows; ++i) {
			var row: ContactSolverInfoRow = this.info.rows[i];
			var md: ContactSolverMassDataRow = this.massData[i];
			var imp: ContactImpulse = row.impulse;
			var j: JacobianRow = row.jacobianN;

			// measure relative velocity
			var rvn: number = 0;
			rvn += M.vec3_dot(lv1, j.lin1);
			rvn -= M.vec3_dot(lv2, j.lin2);
			rvn += M.vec3_dot(av1, j.ang1);
			rvn -= M.vec3_dot(av2, j.ang2);

			var impulseP: number = (row.rhs - rvn) * md.massN * Setting.positionSplitImpulseBaumgarte;

			// clamp impulse
			var oldImpulseP: number = imp.impulseP;
			imp.impulseP += impulseP;
			if (imp.impulseP < 0) imp.impulseP = 0;
			impulseP = imp.impulseP - oldImpulseP;

			// apply delta impulse
			M.vec3_addRhsScaledEQ(lv1, lv1, md.invMLinN1, impulseP);
			M.vec3_addRhsScaledEQ(lv2, lv2, md.invMLinN2, -impulseP);
			M.vec3_addRhsScaledEQ(av1, av1, md.invMAngN1, impulseP);
			M.vec3_addRhsScaledEQ(av2, av2, md.invMAngN2, -impulseP);
		}

		M.vec3_assign(this._b1._pseudoVel, lv1);
		M.vec3_assign(this._b2._pseudoVel, lv2);
		M.vec3_assign(this._b1._angPseudoVel, av1);
		M.vec3_assign(this._b2._angPseudoVel, av2);
	}

	public solvePositionNgs(timeStep: TimeStep): void {
		this.updatePositionData();

		var lv1: Vec3 = new Vec3();
		var lv2: Vec3 = new Vec3();
		var av1: Vec3 = new Vec3();
		var av2: Vec3 = new Vec3();

		for (let i = 0; i < this.info.numRows; ++i) {
			var row: ContactSolverInfoRow = this.info.rows[i];
			var md: ContactSolverMassDataRow = this.massData[i];
			var imp: ContactImpulse = row.impulse;
			var j: JacobianRow = row.jacobianN;

			// estimate translation along the normal
			var rvn: number = 0;
			rvn += M.vec3_dot(lv1, j.lin1);
			rvn -= M.vec3_dot(lv2, j.lin2);
			rvn += M.vec3_dot(av1, j.ang1);
			rvn -= M.vec3_dot(av2, j.ang2);

			var impulseP: number = (row.rhs - rvn) * md.massN * Setting.positionNgsBaumgarte;

			// clamp impulse
			var oldImpulseP: number = imp.impulseP;
			imp.impulseP += impulseP;
			if (imp.impulseP < 0) imp.impulseP = 0;
			impulseP = imp.impulseP - oldImpulseP;

			// apply delta impulse
			M.vec3_addRhsScaledEQ(lv1, lv1, md.invMLinN1, impulseP);
			M.vec3_addRhsScaledEQ(lv2, lv2, md.invMLinN2, -impulseP);
			M.vec3_addRhsScaledEQ(av1, av1, md.invMAngN1, impulseP);
			M.vec3_addRhsScaledEQ(av2, av2, md.invMAngN2, -impulseP);
		}

		(this._b1._applyTranslation(lv1));
		(this._b2._applyTranslation(lv2));
		(this._b1._applyRotation(av1));
		(this._b2._applyRotation(av2));
	}

	public postSolve(): void {
		for (let i = 0; i < this.info.numRows; ++i) {
			var row: ContactSolverInfoRow = this.info.rows[i];
			var imp: ContactImpulse = row.impulse;
			var jt: JacobianRow = row.jacobianT;
			var jb: JacobianRow = row.jacobianB;
			var impulseL: Vec3 = new Vec3();

			M.vec3_addRhsScaledEQ(impulseL, impulseL, jt.lin1, imp.impulseT);
			M.vec3_addRhsScaledEQ(impulseL, impulseL, jb.lin1, imp.impulseB);

			M.vec3_assign(imp.impulseL, impulseL);
		}

		this.constraint._syncManifold();
	}

}
